#include <cv.h>
#include <highgui.h>

#include "HandInterface.hpp"

#include <boost/thread.hpp>

using namespace cv;
using namespace hi;

SingleTrackerFaceActivationRunner::SingleTrackerFaceActivationRunner( Ptr< AbstractTrackDataServer > trackDataServerPtr ) : trackDataServerPtr( trackDataServerPtr ),
 ht(0), hl(0), running(false), runningFaceTracker(false), nextFrameReadyBar(4), computationCompleteBar(4), isHandActive(false), isHandActiveBuffer(false) {
}

SingleTrackerFaceActivationRunner::~SingleTrackerFaceActivationRunner() {
}

void SingleTrackerFaceActivationRunner::run() {
	running = true;
	
	while (running && localise() == -1 ) {}
	if (!running) return;
	
	//set up classifier
	gc = Ptr< AbstractGestureClassifier >( new BoostGestureClassifier( "C0", "C0", hl->getProbabilityTransformer() ) );
	
	//set up tracker
	//paramset3
	ht = HandTracker::init( 10.0, 0.1, 100.0, 10.0,
						   trackingRect, frame, Ptr< Segmenter >( new Segmenter( hl->getProbabilityTransformer() ) ) );
	
	// -- run tracker and classifier -- //
	boost::thread trackerThread( boost::bind( &SingleTrackerFaceActivationRunner::runTracker, this ) );
	boost::thread classifierThread( boost::bind( &SingleTrackerFaceActivationRunner::runClassifier, this ) );
	
	// -- run face tracker -- //
	//prepare face tracker
	ft = new FaceTracker( hl->getProbabilityTransformer() );
	faceTrackingRect = trackingRect; //copy rect
	runningFaceTracker = true;
	boost::thread faceTrackerThread( boost::bind( &SingleTrackerFaceActivationRunner::runFaceTracker, this ) );
	
	bool initialDeactivation = true;
	
	while (running) {
		trackingRectBuffer = trackingRect;
		faceTrackingRectBuffer = faceTrackingRect;
		isHandActiveBuffer = isHandActive;
		setNextFrame();
		nextFrameReadyBar.wait(); //allow tracking of next rect to start while processing data from previous rect
		
		//tracking post-processing and data queue for send
		if (initialDeactivation) {
			initialDeactivation = rectanglesIntersect( trackingRectBuffer, faceTrackingRectBuffer ); //disable tracker while on face
			runningFaceTracker = initialDeactivation;
		} else {
			trackDataServerPtr->putTrackData( isHandActiveBuffer, trackingRectBuffer.x+trackingRectBuffer.width/2, trackingRectBuffer.y+trackingRectBuffer.height/2 );
		}

		computationCompleteBar.wait(); //wait for tracking to complete
//		HiVisualiser::refreshWindow("handtracker");
//		waitKey(1);//helps visualiser run more smoothly in some very strange way
	}
}

void SingleTrackerFaceActivationRunner::stop() {
	runningFaceTracker = false;
	running = false;
}

// -- protected methods -- //
void SingleTrackerFaceActivationRunner::runTracker() {
	while (running) {
		nextFrameReadyBar.wait();
		
		trackingRect = ht->track( frame );
		
		computationCompleteBar.wait();
	}
}

void SingleTrackerFaceActivationRunner::runClassifier() {
	while (running) {
		nextFrameReadyBar.wait();
		
		Mat handImg( prevFrame, trackingRectBuffer );//NOTE: uses trackingRectBuffer for thread safety
		
		isHandActive = gc->isHandActive( handImg );
		
		computationCompleteBar.wait();
	}
}

void SingleTrackerFaceActivationRunner::runFaceTracker() {
	while (runningFaceTracker) {
		nextFrameReadyBar.wait();
		
		ft->track( frame, faceTrackingRect );
		
		//		//VISUALISATION
		//		if ( HiVisualiser::windowMatrixMap.find("handtracker") != HiVisualiser::windowMatrixMap.end() && !HiVisualiser::windowMatrixMap["handtracker"].empty()) {
		//			rectangle( HiVisualiser::windowMatrixMap["handtracker"], Point( faceTrackingRect.x, faceTrackingRect.y ),
		//					  Point( faceTrackingRect.x + faceTrackingRect.width, faceTrackingRect.y + faceTrackingRect.height ),
		//					  Scalar(255,0,0) );
		//		}
		
		computationCompleteBar.wait();
	}
	
	nextFrameReadyBar.decrement();
	computationCompleteBar.decrement();
}

int SingleTrackerFaceActivationRunner::localise() {
	hl = new HandLocaliser();
	try {
		setNextFrame();
		trackingRect = hl->localiseFace( frame );
		//trackingRect = hl->localiseHand( frame );
		cout << "Face found" << endl;
	}
	catch (cv::Exception e) {
		if ( e.code == HI_ERR_NOFACEFOUND ) {
			return -1;
		} else {
			throw;
		}
	}
	//forcing rectangle to stay inside frame
	forceInside( frame.size(), trackingRect );
	return 0;
}


